#include "shoot.h"

#include "base_drv/motor_ctrl.h"
#include "base_drv/drv_conf.h"
#include "base_drv/referee.h"
#include "algorithm/pid.h"
#include "algorithm/filter.h"


#include "pid_parameters.h"
#include "fire_control.h"

#include <stdlib.h>
#include <string.h>
#include "trice.h"
#define TRICE_FILE Id(42694)

pid_struct_t shoot_pid[2],bullet_push_pid;
RAM_PERSIST  uint16_t pusher_stuck_cnt;
RAM_PERSIST uint8_t fric_state = 0;

extern uint8_t shoot_allow;
extern CAN_HandleTypeDef SHOOT_MOTORS_HCAN;

HAL_StatusTypeDef set_multi_shoot_output(float shoot_ref_speed, int16_t bullet_pusher_ref_speed);

void shoot_control_loop(rc_ctrl_t* rc_recv){
	if(get_motor_rx_flags(SHOOT_MOTORS_MASKS) == SHOOT_MOTORS_MASKS){
		//clear_motor_rx_flags(SHOOT_MOTORS_MASKS);
		
		if(is_rc_offline())
			{		
			shut_motors_output(&(SHOOT_MOTORS_HCAN),C620_ID_BASE);
			}
		if(rc_recv->rc.switch_right == RC_SW_DOWN)	
		{
			if(rc_recv->rc.switch_left==RC_SW_MID)
			{
				set_multi_shoot_output(16.0f,0);
			}
			else if(rc_recv->rc.switch_left==RC_SW_UP)
			{
				set_multi_shoot_output(16.0f,500);
			}
			else
			{
				shut_motors_output(&(SHOOT_MOTORS_HCAN),C620_ID_BASE);
			}
		}
		else if(rc_recv->rc.switch_right == RC_SW_MID){

			if(rc_recv->rc.switch_left == RC_SW_DOWN){
				
				shut_motors_output(&(SHOOT_MOTORS_HCAN),C620_ID_BASE);
				
			}else if(rc_recv->rc.switch_left == RC_SW_MID){
				
				set_multi_shoot_output(28.0f,0);
				
			}else if(rc_recv->rc.switch_left == RC_SW_UP){
				
				set_multi_shoot_output(28.0f,PUSHER_WHEEL_SPEED);
			}
		}else if(rc_recv->rc.switch_right == RC_SW_UP){
			
			if(rc_recv->rc.switch_left == RC_SW_DOWN)
			{
				shut_motors_output(&(SHOOT_MOTORS_HCAN),C620_ID_BASE);
			}
			else if (rc_recv->rc.switch_left == RC_SW_MID) 
			{
				set_multi_shoot_output(28.0f,0);
			}
			else if (rc_recv->rc.switch_left == RC_SW_UP) 
			{
//				if(shoot_allow == 1)
//				{
//					set_multi_shoot_output(28.0f,PUSHER_WHEEL_SPEED);
//				}
//				else 
//				{
//					set_multi_shoot_output(28.0f,0);
//				}
				set_multi_shoot_output(28.0f,PUSHER_WHEEL_SPEED);
				get_suggest_fire(shoot_allow);
			}
			
		}else{
			shut_motors_output(&(SHOOT_MOTORS_HCAN),C620_ID_BASE);
		}
	}else{
		shut_motors_output(&(SHOOT_MOTORS_HCAN),C620_ID_BASE);
	}
}


void shoot_pid_init(void){
	memcpy(&shoot_pid[0],&shoot_fric_pid_init_val,sizeof(pid_struct_t));
	memcpy(&shoot_pid[1],&shoot_fric_pid_init_val,sizeof(pid_struct_t));

	memcpy(&bullet_push_pid,&bullet_push_pid_init_val,sizeof(pid_struct_t));
	pusher_stuck_cnt = 0;
}

int16_t test_fric[2];
HAL_StatusTypeDef set_multi_shoot_output(float shoot_ref_speed, int16_t bullet_pusher_ref_speed){
	motor_response_msg_t* all_motor_data;
	motor_transmit_msg_t tx_msg;
	int16_t current_pusher_rpm;
	float fric_ref_speed;
	float fric_left_filtered,fric_right_filtered;

	all_motor_data = get_motor_data_ptr();
	memset(tx_msg.D,0,sizeof(motor_transmit_msg_t));
	
	if(shoot_ref_speed > 0 && shoot_ref_speed < MAX_SHOOT_SPEED){
		//fric_ref_speed = SHOOT_SPEED_DEC * (float)shoot_ref_speed / C_FRIC_WHEEL * 60;
		fric_ref_speed = 6800;
		fric_left_filtered = iir_filter_2(all_motor_data[CAN_SHOOT_LEFT_ID].speed_rpm,SHOOT_LEFT_FILTER2_RPM_CH);
		fric_right_filtered = iir_filter_2(all_motor_data[CAN_SHOOT_RIGHT_ID].speed_rpm,SHOOT_RIGHT_FILTER2_RPM_CH);
		
		test_fric[0] = all_motor_data[CAN_SHOOT_LEFT_ID].speed_rpm;
		test_fric[1] = -all_motor_data[CAN_SHOOT_RIGHT_ID].speed_rpm;
		
		tx_msg.D[1] = (int16_t)pid_calc(&shoot_pid[0], fric_ref_speed, fric_left_filtered);
		tx_msg.D[0] = (int16_t)pid_calc(&shoot_pid[1], -fric_ref_speed, fric_right_filtered);
	}else{
		tx_msg.D[1] = 0; //CAN_SHOOT_LEFT_ID
		tx_msg.D[0] = 0; //CAN_SHOOT_RIGHT_ID
	}

//	current_pusher_rpm = all_motor_data[CAN_BULLET_PUSHER_ID].speed_rpm;
//	int16_t pusher_current = all_motor_data[CAN_BULLET_PUSHER_ID].current;
//	//anti-stuck process
//	int16_t remaining_heat = game_robot_status.shooter_id1_17mm_cooling_limit - power_heat_data.shooter_id1_17mm_cooling_heat;
//	if(remaining_heat > MIN_SHOOT_HEAT || game_robot_status.shooter_id1_17mm_cooling_limit == 65535)
//	{
//	if(abs(current_pusher_rpm) < STUCK_SPEED 
//		&& bullet_pusher_ref_speed > STUCK_SPEED
//		&& abs(pusher_current) > 2500){
//			if(pusher_stuck_cnt < STUCK_MAX_TIME_CNT){
//				pusher_stuck_cnt++;
//			}else if(pusher_stuck_cnt == STUCK_MAX_TIME_CNT){
//				pusher_stuck_cnt += STUCK_REVERSE_TIME_CNT;
//				TRICE( Id(43809),"WRN: bullet stuck detect, try reversed\n");
//			}
//	}else if(pusher_stuck_cnt <= STUCK_MAX_TIME_CNT){
//		pusher_stuck_cnt = 0;
//	}

//	if(pusher_stuck_cnt > STUCK_MAX_TIME_CNT){
//		//direction( + or -?)
//		bullet_pusher_ref_speed = STUCK_REVERSE_RPM;
//		pusher_stuck_cnt--;
//	}
//		tx_msg.D[2] = (int16_t)pid_calc(&bullet_push_pid, -bullet_pusher_ref_speed, current_pusher_rpm);
//	}else{
//		bullet_pusher_ref_speed = 0;	
//		tx_msg.D[2] = (int16_t)pid_calc(&bullet_push_pid, bullet_pusher_ref_speed, current_pusher_rpm);   
//	}
	
	return set_motor_output(&(SHOOT_MOTORS_HCAN),&tx_msg,C620_ID_BASE);
	
}

float get_bullet_inital_speed(int16_t bullet_pusher_ref_speed){
	return C_PUSHER_WHEEL*(bullet_pusher_ref_speed/M2006_REDUCTION_RATIO)/60;
}

